
import rospy
import tf
import numpy as np
from geometry_msgs.msg import PoseArray,PoseStamped,Pose,PoseWithCovarianceStamped,TransformStamped
from ros_numpy import numpify,msgify
from nav_msgs.msg import Path
from tf.transformations import quaternion_from_matrix
import math
from sensor_msgs.msg import PointCloud2,PointCloud
import tf2_ros


def calculate_angle1(vector1, vector2):
    dot_product = sum(x * y for x, y in zip(vector1, vector2))
    magnitude1 = math.sqrt(sum(x**2 for x in vector1))
    magnitude2 = math.sqrt(sum(x**2 for x in vector2))
    
    cosine_angle = dot_product / (magnitude1 * magnitude2)
    angle_in_radians = math.acos(cosine_angle)
    angle_in_degrees = math.degrees(angle_in_radians)